
/** \file
 *
 *  ROS driver interface for the lidar209 3D LIDARs
 */

#ifndef _lidar209_DRIVER_H_
#define _lidar209_DRIVER_H_ 1

#include <diagnostic_updater/diagnostic_updater.hpp>
#include <diagnostic_updater/publisher.hpp>
#include <rclcpp/rclcpp.hpp>

#include <future>
#include <memory>
#include <string>

#include <lidar209_msgs/msg/lidar209_scan_ch88.hpp>
#include <lidar209_msgs/msg/lidar209_status_ch176.hpp>

#include "input.h"

namespace lidar209_driver
{

class lidar209Driver final : public rclcpp::Node
{
public:
  explicit lidar209Driver(const rclcpp::NodeOptions & options);
  ~lidar209Driver() override;
  lidar209Driver(lidar209Driver && c) = delete;
  lidar209Driver & operator=(lidar209Driver && c) = delete;
  lidar209Driver(const lidar209Driver & c) = delete;
  lidar209Driver & operator=(const lidar209Driver & c) = delete;


  typedef  unsigned char  byte;

  static const int Num_Of_Detector_CH88=88;
  static const int RAW_SCAN_SIZE = 3;
  static const int SIZE_BLOCK_CH88 = 69;
  static const int SCANS_PER_BLOCK_CH88 = 22;
  static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK_CH88 * RAW_SCAN_SIZE);

  typedef struct raw_block_CH88
  {
    uint8_t header;        ///< UPPER_BANK or LOWER_BANK
    byte rotation[2];      ///< 0-35999, divide by 100 to get degrees
    uint8_t  data[BLOCK_DATA_SIZE];
  } raw_block_t_CH88;


  static const int PACKET_SIZE = 1449;
  static const int BLOCKS_PER_PACKET = 20;
  static const int SCANS_PER_PACKET = (SCANS_PER_BLOCK_CH88 * BLOCKS_PER_PACKET);

  typedef struct raw_packet_CH88
  {
    raw_block_t_CH88 blocks[BLOCKS_PER_PACKET];
    byte revolution[4];
    uint8_t type;
    uint8_t status2[64];
  } raw_packet_t_CH88;

  int bytes4ToInt(byte *b);

  bool poll_in_ch88(void);

  double packet_rate;                   // packet frequency (Hz)

  void pollThread();

private:

  // configuration parameters
  struct
  {
    std::string frame_id;            ///< tf frame ID
    std::string model;               ///< device model name
    int    npackets;                 ///< number of packets to collect
    double rpm;                      ///< device rotation rate (RPMs)
    //double cut_angle;                   ///< cutting angle in 1/100°
    int cut_time;                   ///< cutting time
    double time_offset;              ///< time in seconds added to each lidar209 time stamp
    bool enabled;                    // polling is enabled
  } config_;


  // //
  std::unique_ptr<Input> input_;
  rclcpp::Publisher<lidar209_msgs::msg::Lidar209ScanCH88>::SharedPtr output_;
  rclcpp::Publisher<lidar209_msgs::msg::Lidar209StatusCH176>::SharedPtr status_output_;

  int last_azimuth_;

  /* diagnostics updater */
  diagnostic_updater::Updater diagnostics_;
  double diag_min_freq_;
  double diag_max_freq_;
  int udp_packets_num;
  int last_scan_time;
  int time_error;
  std::unique_ptr<diagnostic_updater::TopicDiagnostic> diag_topic_;

  // We use this future/promise pair to notify threads that we are shutting down
  std::shared_future<void> future_;
  std::promise<void> exit_signal_;

  // The thread that deals with data
  std::thread poll_thread_;
};

} // namespace lidar209_driver

#endif // _lidar209_DRIVER_H_
